package robotutils.planning;

import java.io.Serializable;
import java.util.Comparator;


import robotutils.util.PriorityQueue;

/**
 * A simple wrapper to a priority queue that internally stores a tuple of
 * State and Key with the correct comparison and equals operators.  This
 * cheaply implements the lookup behavior expected by D*.
 */
public class KeyQueue<State> implements Serializable {

    /**
	 * 
	 */
	private static final long serialVersionUID = 1L;

	/**
     * Defines a suitably large initial capacity for internal data structures
     * to avoid Java's heuristics reallocating up from scratch.
     */
    public static final int INITIAL_CAPACITY = 1024;
	
	private final class StateKeyComparator implements Comparator<StateKey<State>>, Serializable {
        
        /**
		 * 
		 */
		private static final long serialVersionUID = 1L;

		public final int compare(StateKey<State> o1, StateKey<State> o2) {
            // Lowest key = highest priority (reversing lexical ordering)
            return o1.k.compareTo(o2.k);
        }
    }

    private PriorityQueue<StateKey<State>> _queue;
    
    public KeyQueue() {
        _queue = new PriorityQueue(INITIAL_CAPACITY, new StateKeyComparator());
    }

    public void insert(State s, Key k) {
        _queue.add(new StateKey<State>(s, k));
    }

    public void update(State s, Key k) {
        _queue.update(new StateKey<State>(s, k));
    }

    public void remove(State s) {
        _queue.remove(new StateKey<State>(s, null));
    }

    public Key topKey() {
        return _queue.peek().k;
    }

    public State top() {
        return _queue.peek().s;
    }

    public boolean contains(State s) {
        return _queue.contains(new StateKey<State>(s, null));
    }

    public boolean isEmpty() {
        return _queue.isEmpty();
    }

    @Override
    public String toString() {
        String str = "[";
        PriorityQueue tmp = new PriorityQueue(_queue);

        while (!tmp.isEmpty()) {
            str += tmp.poll().toString();
            if (!tmp.isEmpty()) str += ",";
        }
        
        return str + "]";
    }
}